<!-- saved from url=(0014)about:internet -->
<pre class="code">
<span class="srcline"><span class="lineno"><a href="1,1" id="srcline1">  1</a></span><span class="line"><span class="keyword">function</span> [<span class="var type1" id="S2T12U3">xa_apo</span>,<span class="var type1" id="S3T24U4">Pa_apo</span>,<span class="var type1" id="S4T2U5">Rot_matrix</span>,<span class="var type1" id="S5T10U6">eulerAngles</span>,<span class="var type1" id="S6T46U7">debugOutput</span>]<span class="keyword">...</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,2" id="srcline2">  2</a></span><span class="line">    = AttitudeEKF(<span class="var type1" id="S7T13U10">approx_prediction</span>,<span class="var type1" id="S8T13U11">use_inertia_matrix</span>,<span class="var type1" id="S9T26U12">zFlag</span>,<span class="var type1" id="S10T5U13">dt</span>,<span class="var type1" id="S11T27U14">z</span>,<span class="var type1" id="S12T5U15">q_rotSpeed</span>,<span class="var type1" id="S13T5U16">q_rotAcc</span>,<span class="var type1" id="S14T5U17">q_acc</span>,<span class="var type1" id="S15T5U18">q_mag</span>,<span class="var type1" id="S16T5U19">r_gyro</span>,<span class="var type1" id="S17T5U20">r_accel</span>,<span class="var type1" id="S18T5U21">r_mag</span>,<span class="var type1" id="S19T2U22">J</span>)</span></span>
<span class="srcline"><span class="lineno"><a href="1,3" id="srcline3">  3</a></span><span class="line"></span></span>
<span class="srcline"><span class="lineno"><a href="1,4" id="srcline4">  4</a></span><span class="line"></span></span>
<span class="srcline"><span class="lineno"><a href="1,5" id="srcline5">  5</a></span><span class="line"><span class="comment">%LQG Postion Estimator and Controller</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,6" id="srcline6">  6</a></span><span class="line"><span class="comment">% Observer:</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,7" id="srcline7">  7</a></span><span class="line"><span class="comment">%        x[n|n]   = x[n|n-1] + M(y[n] - Cx[n|n-1] - Du[n])</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,8" id="srcline8">  8</a></span><span class="line"><span class="comment">%        x[n+1|n] = Ax[n|n] + Bu[n]</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,9" id="srcline9">  9</a></span><span class="line"><span class="comment">%</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,10" id="srcline10"> 10</a></span><span class="line"><span class="comment">% $Author: Tobias Naegeli $    $Date: 2014 $    $Revision: 3 $</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,11" id="srcline11"> 11</a></span><span class="line"><span class="comment">%</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,12" id="srcline12"> 12</a></span><span class="line"><span class="comment">%</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,13" id="srcline13"> 13</a></span><span class="line"><span class="comment">% Arguments:</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,14" id="srcline14"> 14</a></span><span class="line"><span class="comment">% approx_prediction: if 1 then the exponential map is approximated with a</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,15" id="srcline15"> 15</a></span><span class="line"><span class="comment">% first order taylor approximation. has at the moment not a big influence</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,16" id="srcline16"> 16</a></span><span class="line"><span class="comment">% (just 1st or 2nd order approximation) we should change it to rodriquez</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,17" id="srcline17"> 17</a></span><span class="line"><span class="comment">% approximation.</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,18" id="srcline18"> 18</a></span><span class="line"><span class="comment">% use_inertia_matrix: set to true if you have the inertia matrix J for your</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,19" id="srcline19"> 19</a></span><span class="line"><span class="comment">% quadrotor</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,20" id="srcline20"> 20</a></span><span class="line"><span class="comment">% xa_apo_k: old state vectotr</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,21" id="srcline21"> 21</a></span><span class="line"><span class="comment">% zFlag: if sensor measurement is available [gyro, acc, mag]</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,22" id="srcline22"> 22</a></span><span class="line"><span class="comment">% dt: dt in s</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,23" id="srcline23"> 23</a></span><span class="line"><span class="comment">% z: measurements [gyro, acc, mag]</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,24" id="srcline24"> 24</a></span><span class="line"><span class="comment">% q_rotSpeed: process noise gyro</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,25" id="srcline25"> 25</a></span><span class="line"><span class="comment">% q_rotAcc: process noise gyro acceleration</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,26" id="srcline26"> 26</a></span><span class="line"><span class="comment">% q_acc: process noise acceleration</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,27" id="srcline27"> 27</a></span><span class="line"><span class="comment">% q_mag: process noise magnetometer</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,28" id="srcline28"> 28</a></span><span class="line"><span class="comment">% r_gyro: measurement noise gyro</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,29" id="srcline29"> 29</a></span><span class="line"><span class="comment">% r_accel: measurement noise accel</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,30" id="srcline30"> 30</a></span><span class="line"><span class="comment">% r_mag: measurement noise mag</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,31" id="srcline31"> 31</a></span><span class="line"><span class="comment">% J: moment of inertia matrix</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,32" id="srcline32"> 32</a></span><span class="line"></span></span>
<span class="srcline"><span class="lineno"><a href="1,33" id="srcline33"> 33</a></span><span class="line"></span></span>
<span class="srcline"><span class="lineno"><a href="1,34" id="srcline34"> 34</a></span><span class="line"><span class="comment">% Output:</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,35" id="srcline35"> 35</a></span><span class="line"><span class="comment">% xa_apo: updated state vectotr</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,36" id="srcline36"> 36</a></span><span class="line"><span class="comment">% Pa_apo: updated state covariance matrix</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,37" id="srcline37"> 37</a></span><span class="line"><span class="comment">% Rot_matrix: rotation matrix</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,38" id="srcline38"> 38</a></span><span class="line"><span class="comment">% eulerAngles: euler angles</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,39" id="srcline39"> 39</a></span><span class="line"><span class="comment">% debugOutput: not used</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,40" id="srcline40"> 40</a></span><span class="line"></span></span>
<span class="srcline"><span class="lineno"><a href="1,41" id="srcline41"> 41</a></span><span class="line"></span></span>
<span class="srcline"><span class="lineno"><a href="1,42" id="srcline42"> 42</a></span><span class="line"><span class="comment">%% model specific parameters</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,43" id="srcline43"> 43</a></span><span class="line"></span></span>
<span class="srcline"><span class="lineno"><a href="1,44" id="srcline44"> 44</a></span><span class="line"></span></span>
<span class="srcline"><span class="lineno"><a href="1,45" id="srcline45"> 45</a></span><span class="line"></span></span>
<span class="srcline"><span class="lineno"><a href="1,46" id="srcline46"> 46</a></span><span class="line"><span class="comment">% compute once the inverse of the Inertia</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,47" id="srcline47"> 47</a></span><span class="line"><span class="keyword">persistent</span> <span class="var type1" id="S20T2U24">Ji</span>;</span></span>
<span class="srcline"><span class="lineno"><a href="1,48" id="srcline48"> 48</a></span><span class="line"><span class="keyword">if</span> <span class="mxinfo" id="T1:U20"><span class="mxinfo" id="T1:U21">isempty(<span class="var type0" id="S20T0U29">Ji</span>)</span></span></span></span>
<span class="srcline"><span class="lineno"><a href="1,49" id="srcline49"> 49</a></span><span class="line">    <span class="mxinfo" id="T2:U22"><span class="var type1" id="S20T2U32">Ji</span>=single(<span class="mxinfo" id="T2:U24">inv(<span class="var type1" id="S19T2U37">J</span>)</span>)</span>;</span></span>
<span class="srcline"><span class="lineno"><a href="1,50" id="srcline50"> 50</a></span><span class="line"><span class="keyword">end</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,51" id="srcline51"> 51</a></span><span class="line"></span></span>
<span class="srcline"><span class="lineno"><a href="1,52" id="srcline52"> 52</a></span><span class="line"><span class="comment">%% init</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,53" id="srcline53"> 53</a></span><span class="line"><span class="keyword">persistent</span> <span class="var type1" id="S24T12U39">x_apo</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,54" id="srcline54"> 54</a></span><span class="line"><span class="keyword">if</span>(<span class="mxinfo" id="T1:U27"><span class="mxinfo" id="T1:U28">isempty(<span class="var type0" id="S24T0U45">x_apo</span>)</span></span>)</span></span>
<span class="srcline"><span class="lineno"><a href="1,55" id="srcline55"> 55</a></span><span class="line">    <span class="mxinfo" id="T10:U29"><span class="var type1" id="S25T10U48">gyro_init</span>=<span class="mxinfo" id="T10:U31">single([0;0;0])</span></span>;</span></span>
<span class="srcline"><span class="lineno"><a href="1,56" id="srcline56"> 56</a></span><span class="line">    <span class="mxinfo" id="T10:U32"><span class="var type1" id="S26T10U60">gyro_acc_init</span>=<span class="mxinfo" id="T10:U34">single([0;0;0])</span></span>;</span></span>
<span class="srcline"><span class="lineno"><a href="1,57" id="srcline57"> 57</a></span><span class="line">    <span class="mxinfo" id="T10:U35"><span class="var type1" id="S27T10U72">acc_init</span>=<span class="mxinfo" id="T10:U37">single([0;0;-9.81])</span></span>;</span></span>
<span class="srcline"><span class="lineno"><a href="1,58" id="srcline58"> 58</a></span><span class="line">    <span class="mxinfo" id="T10:U38"><span class="var type1" id="S28T10U85">mag_init</span>=<span class="mxinfo" id="T10:U40">single([1;0;0])</span></span>;</span></span>
<span class="srcline"><span class="lineno"><a href="1,59" id="srcline59"> 59</a></span><span class="line">    <span class="mxinfo" id="T12:U41"><span class="var type1" id="S24T12U97">x_apo</span>=single(<span class="mxinfo" id="T12:U43">[<span class="var type1" id="S25T10U102">gyro_init</span>;<span class="var type1" id="S26T10U104">gyro_acc_init</span>;<span class="var type1" id="S27T10U106">acc_init</span>;<span class="var type1" id="S28T10U108">mag_init</span>]</span>)</span>;</span></span>
<span class="srcline"><span class="lineno"><a href="1,60" id="srcline60"> 60</a></span><span class="line">    </span></span>
<span class="srcline"><span class="lineno"><a href="1,61" id="srcline61"> 61</a></span><span class="line"><span class="keyword">end</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,62" id="srcline62"> 62</a></span><span class="line"></span></span>
<span class="srcline"><span class="lineno"><a href="1,63" id="srcline63"> 63</a></span><span class="line"><span class="keyword">persistent</span> <span class="var type1" id="S29T24U110">P_apo</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,64" id="srcline64"> 64</a></span><span class="line"><span class="keyword">if</span>(<span class="mxinfo" id="T1:U49"><span class="mxinfo" id="T1:U50">isempty(<span class="var type0" id="S29T0U116">P_apo</span>)</span></span>)</span></span>
<span class="srcline"><span class="lineno"><a href="1,65" id="srcline65"> 65</a></span><span class="line">    <span class="comment">%     P_apo = single(eye(NSTATES) * 1000);</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,66" id="srcline66"> 66</a></span><span class="line">    <span class="mxinfo" id="T24:U51"><span class="var type1" id="S29T24U119">P_apo</span> = <span class="mxinfo" id="T24:U53">single(<span class="mxinfo" id="T11:U54">200*ones(12)</span>)</span></span>;</span></span>
<span class="srcline"><span class="lineno"><a href="1,67" id="srcline67"> 67</a></span><span class="line"><span class="keyword">end</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,68" id="srcline68"> 68</a></span><span class="line"></span></span>
<span class="srcline"><span class="lineno"><a href="1,69" id="srcline69"> 69</a></span><span class="line"><span class="mxinfo" id="T46:U55"><span class="var type1" id="S6T46U129">debugOutput</span> = <span class="mxinfo" id="T46:U57">single(<span class="mxinfo" id="T47:U58">zeros(4,1)</span>)</span></span>;</span></span>
<span class="srcline"><span class="lineno"><a href="1,70" id="srcline70"> 70</a></span><span class="line"></span></span>
<span class="srcline"><span class="lineno"><a href="1,71" id="srcline71"> 71</a></span><span class="line"><span class="comment">%% copy the states</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,72" id="srcline72"> 72</a></span><span class="line"><span class="mxinfo" id="T5:U59"><span class="var type1" id="S32T5U138">wx</span>=  <span class="mxinfo" id="T5:U61"><span class="var type1" id="S24T12U140">x_apo</span>(<span class="mxinfo" id="T6:U63">1</span>)</span></span>;   <span class="comment">% x  body angular rate</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,73" id="srcline73"> 73</a></span><span class="line"><span class="mxinfo" id="T5:U64"><span class="var type1" id="S33T5U144">wy</span>=  <span class="mxinfo" id="T5:U66"><span class="var type1" id="S24T12U146">x_apo</span>(<span class="mxinfo" id="T6:U68">2</span>)</span></span>;   <span class="comment">% y  body angular rate</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,74" id="srcline74"> 74</a></span><span class="line"><span class="mxinfo" id="T5:U69"><span class="var type1" id="S34T5U150">wz</span>=  <span class="mxinfo" id="T5:U71"><span class="var type1" id="S24T12U152">x_apo</span>(<span class="mxinfo" id="T6:U73">3</span>)</span></span>;   <span class="comment">% z  body angular rate</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,75" id="srcline75"> 75</a></span><span class="line"></span></span>
<span class="srcline"><span class="lineno"><a href="1,76" id="srcline76"> 76</a></span><span class="line"><span class="mxinfo" id="T5:U74"><span class="var type1" id="S35T5U156">wax</span>=  <span class="mxinfo" id="T5:U76"><span class="var type1" id="S24T12U158">x_apo</span>(<span class="mxinfo" id="T6:U78">4</span>)</span></span>;  <span class="comment">% x  body angular acceleration</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,77" id="srcline77"> 77</a></span><span class="line"><span class="mxinfo" id="T5:U79"><span class="var type1" id="S36T5U162">way</span>=  <span class="mxinfo" id="T5:U81"><span class="var type1" id="S24T12U164">x_apo</span>(<span class="mxinfo" id="T6:U83">5</span>)</span></span>;  <span class="comment">% y  body angular acceleration</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,78" id="srcline78"> 78</a></span><span class="line"><span class="mxinfo" id="T5:U84"><span class="var type1" id="S37T5U168">waz</span>=  <span class="mxinfo" id="T5:U86"><span class="var type1" id="S24T12U170">x_apo</span>(<span class="mxinfo" id="T6:U88">6</span>)</span></span>;  <span class="comment">% z  body angular acceleration</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,79" id="srcline79"> 79</a></span><span class="line"></span></span>
<span class="srcline"><span class="lineno"><a href="1,80" id="srcline80"> 80</a></span><span class="line"><span class="mxinfo" id="T5:U89"><span class="var type1" id="S38T5U174">zex</span>=  <span class="mxinfo" id="T5:U91"><span class="var type1" id="S24T12U176">x_apo</span>(<span class="mxinfo" id="T6:U93">7</span>)</span></span>;  <span class="comment">% x  component gravity vector</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,81" id="srcline81"> 81</a></span><span class="line"><span class="mxinfo" id="T5:U94"><span class="var type1" id="S39T5U180">zey</span>=  <span class="mxinfo" id="T5:U96"><span class="var type1" id="S24T12U182">x_apo</span>(<span class="mxinfo" id="T6:U98">8</span>)</span></span>;  <span class="comment">% y  component gravity vector</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,82" id="srcline82"> 82</a></span><span class="line"><span class="mxinfo" id="T5:U99"><span class="var type1" id="S40T5U186">zez</span>=  <span class="mxinfo" id="T5:U101"><span class="var type1" id="S24T12U188">x_apo</span>(<span class="mxinfo" id="T6:U103">9</span>)</span></span>;  <span class="comment">% z  component gravity vector</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,83" id="srcline83"> 83</a></span><span class="line"></span></span>
<span class="srcline"><span class="lineno"><a href="1,84" id="srcline84"> 84</a></span><span class="line"><span class="mxinfo" id="T5:U104"><span class="var type1" id="S41T5U192">mux</span>=  <span class="mxinfo" id="T5:U106"><span class="var type1" id="S24T12U194">x_apo</span>(<span class="mxinfo" id="T6:U108">10</span>)</span></span>; <span class="comment">% x  component magnetic field vector</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,85" id="srcline85"> 85</a></span><span class="line"><span class="mxinfo" id="T5:U109"><span class="var type1" id="S42T5U198">muy</span>=  <span class="mxinfo" id="T5:U111"><span class="var type1" id="S24T12U200">x_apo</span>(<span class="mxinfo" id="T6:U113">11</span>)</span></span>; <span class="comment">% y  component magnetic field vector</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,86" id="srcline86"> 86</a></span><span class="line"><span class="mxinfo" id="T5:U114"><span class="var type1" id="S43T5U204">muz</span>=  <span class="mxinfo" id="T5:U116"><span class="var type1" id="S24T12U206">x_apo</span>(<span class="mxinfo" id="T6:U118">12</span>)</span></span>; <span class="comment">% z  component magnetic field vector</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,87" id="srcline87"> 87</a></span><span class="line"></span></span>
<span class="srcline"><span class="lineno"><a href="1,88" id="srcline88"> 88</a></span><span class="line"></span></span>
<span class="srcline"><span class="lineno"><a href="1,89" id="srcline89"> 89</a></span><span class="line"></span></span>
<span class="srcline"><span class="lineno"><a href="1,90" id="srcline90"> 90</a></span><span class="line"></span></span>
<span class="srcline"><span class="lineno"><a href="1,91" id="srcline91"> 91</a></span><span class="line"><span class="comment">%% prediction section</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,92" id="srcline92"> 92</a></span><span class="line"><span class="comment">% compute the apriori state estimate from the previous aposteriori estimate</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,93" id="srcline93"> 93</a></span><span class="line"><span class="comment">%body angular accelerations</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,94" id="srcline94"> 94</a></span><span class="line"><span class="keyword">if</span> (<span class="mxinfo" id="T1:U119"><span class="var type1" id="S8T13U212">use_inertia_matrix</span>==<span class="mxinfo" id="T6:U121">1</span></span>)</span></span>
<span class="srcline"><span class="lineno"><a href="1,95" id="srcline95"> 95</a></span><span class="line">    <span class="mxinfo" id="T10:U122"><span class="var type1" id="S44T10U216">wak</span> =<span class="mxinfo" id="T10:U124"><span class="mxinfo" id="T10:U125">[<span class="var type1" id="S35T5U220">wax</span>;<span class="var type1" id="S36T5U222">way</span>;<span class="var type1" id="S37T5U224">waz</span>]</span>+<span class="mxinfo" id="T10:U129"><span class="mxinfo" id="T10:U130"><span class="var type1" id="S20T2U227">Ji</span>*(<span class="mxinfo" id="T10:U132">-<span class="mxinfo" id="T10:U133">cross(<span class="mxinfo" id="T10:U134">[<span class="var type1" id="S35T5U234">wax</span>;<span class="var type1" id="S36T5U236">way</span>;<span class="var type1" id="S37T5U238">waz</span>]</span>,<span class="mxinfo" id="T10:U138"><span class="var type1" id="S19T2U240">J</span>*<span class="mxinfo" id="T10:U140">[<span class="var type1" id="S35T5U243">wax</span>;<span class="var type1" id="S36T5U245">way</span>;<span class="var type1" id="S37T5U247">waz</span>]</span></span>)</span></span>)</span>*<span class="var type1" id="S10T5U248">dt</span></span></span></span>;</span></span>
<span class="srcline"><span class="lineno"><a href="1,96" id="srcline96"> 96</a></span><span class="line"><span class="keyword">else</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,97" id="srcline97"> 97</a></span><span class="line">    <span class="mxinfo" id="T10:U145"><span class="var type1" id="S44T10U252">wak</span> =<span class="mxinfo" id="T10:U147">[<span class="var type1" id="S35T5U255">wax</span>;<span class="var type1" id="S36T5U257">way</span>;<span class="var type1" id="S37T5U259">waz</span>]</span></span>;</span></span>
<span class="srcline"><span class="lineno"><a href="1,98" id="srcline98"> 98</a></span><span class="line"><span class="keyword">end</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,99" id="srcline99"> 99</a></span><span class="line"></span></span>
<span class="srcline"><span class="lineno"><a href="1,100" id="srcline100">100</a></span><span class="line"><span class="comment">%body angular rates</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,101" id="srcline101">101</a></span><span class="line"><span class="mxinfo" id="T10:U151"><span class="var type1" id="S46T10U262">wk</span> =<span class="mxinfo" id="T10:U153"><span class="mxinfo" id="T10:U154">[<span class="var type1" id="S32T5U266">wx</span>;  <span class="var type1" id="S33T5U268">wy</span>; <span class="var type1" id="S34T5U270">wz</span>]</span> + <span class="mxinfo" id="T10:U158"><span class="var type1" id="S10T5U272">dt</span>*<span class="var type1" id="S44T10U273">wak</span></span></span></span>;</span></span>
<span class="srcline"><span class="lineno"><a href="1,102" id="srcline102">102</a></span><span class="line"></span></span>
<span class="srcline"><span class="lineno"><a href="1,103" id="srcline103">103</a></span><span class="line"><span class="comment">%derivative of the prediction rotation matrix</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,104" id="srcline104">104</a></span><span class="line"><span class="mxinfo" id="T2:U161"><span class="var type1" id="S47T2U276">O</span>=<span class="mxinfo" id="T2:U163"><span class="mxinfo" id="T2:U164">[<span class="mxinfo" id="T39:U165"><span class="mxinfo" id="T6:U166">0</span>,<span class="mxinfo" id="T5:U167">-<span class="var type1" id="S34T5U282">wz</span></span>,<span class="var type1" id="S33T5U283">wy</span></span>;<span class="mxinfo" id="T39:U170"><span class="var type1" id="S34T5U285">wz</span>,<span class="mxinfo" id="T6:U172">0</span>,<span class="mxinfo" id="T5:U173">-<span class="var type1" id="S32T5U288">wx</span></span></span>;<span class="mxinfo" id="T39:U175"><span class="mxinfo" id="T5:U176">-<span class="var type1" id="S33T5U291">wy</span></span>,<span class="var type1" id="S32T5U292">wx</span>,<span class="mxinfo" id="T6:U179">0</span></span>]</span>'</span></span>;</span></span>
<span class="srcline"><span class="lineno"><a href="1,105" id="srcline105">105</a></span><span class="line"></span></span>
<span class="srcline"><span class="lineno"><a href="1,106" id="srcline106">106</a></span><span class="line"><span class="comment">%prediction of the earth z vector</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,107" id="srcline107">107</a></span><span class="line"><span class="keyword">if</span> (<span class="mxinfo" id="T1:U180"><span class="var type1" id="S7T13U298">approx_prediction</span>==<span class="mxinfo" id="T6:U182">1</span></span>)</span></span>
<span class="srcline"><span class="lineno"><a href="1,108" id="srcline108">108</a></span><span class="line">    <span class="comment">%e^(Odt)=I+dt*O+dt^2/2!O^2</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,109" id="srcline109">109</a></span><span class="line">    <span class="comment">% so we do a first order approximation of the exponential map</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,110" id="srcline110">110</a></span><span class="line">    <span class="mxinfo" id="T10:U183"><span class="var type1" id="S48T10U302">zek</span> =<span class="mxinfo" id="T10:U185">(<span class="mxinfo" id="T2:U186"><span class="mxinfo" id="T2:U187"><span class="var type1" id="S47T2U307">O</span>*<span class="var type1" id="S10T5U308">dt</span></span>+<span class="mxinfo" id="T2:U190">single(<span class="mxinfo" id="T21:U191">eye(3)</span>)</span></span>)*<span class="mxinfo" id="T10:U192">[<span class="var type1" id="S38T5U316">zex</span>;<span class="var type1" id="S39T5U318">zey</span>;<span class="var type1" id="S40T5U320">zez</span>]</span></span></span>;</span></span>
<span class="srcline"><span class="lineno"><a href="1,111" id="srcline111">111</a></span><span class="line">    </span></span>
<span class="srcline"><span class="lineno"><a href="1,112" id="srcline112">112</a></span><span class="line"><span class="keyword">else</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,113" id="srcline113">113</a></span><span class="line">    <span class="mxinfo" id="T10:U196"><span class="var type1" id="S48T10U324">zek</span> =<span class="mxinfo" id="T10:U198">(<span class="mxinfo" id="T2:U199"><span class="mxinfo" id="T2:U200"><span class="mxinfo" id="T2:U201">single(<span class="mxinfo" id="T21:U202">eye(3)</span>)</span>+<span class="mxinfo" id="T2:U203"><span class="var type1" id="S47T2U335">O</span>*<span class="var type1" id="S10T5U336">dt</span></span></span>+<span class="mxinfo" id="T2:U206"><span class="mxinfo" id="T5:U207"><span class="mxinfo" id="T5:U208"><span class="var type1" id="S10T5U340">dt</span>^2</span>/<span class="mxinfo" id="T6:U210">2</span></span>*<span class="mxinfo" id="T2:U211"><span class="var type1" id="S47T2U344">O</span>^2</span></span></span>)*<span class="mxinfo" id="T10:U213">[<span class="var type1" id="S38T5U348">zex</span>;<span class="var type1" id="S39T5U350">zey</span>;<span class="var type1" id="S40T5U352">zez</span>]</span></span></span>;</span></span>
<span class="srcline"><span class="lineno"><a href="1,114" id="srcline114">114</a></span><span class="line">    <span class="comment">%zek =expm2(O*dt)*[zex;zey;zez]; not working because use double</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,115" id="srcline115">115</a></span><span class="line">    <span class="comment">%precision</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,116" id="srcline116">116</a></span><span class="line"><span class="keyword">end</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,117" id="srcline117">117</a></span><span class="line"></span></span>
<span class="srcline"><span class="lineno"><a href="1,118" id="srcline118">118</a></span><span class="line"></span></span>
<span class="srcline"><span class="lineno"><a href="1,119" id="srcline119">119</a></span><span class="line"></span></span>
<span class="srcline"><span class="lineno"><a href="1,120" id="srcline120">120</a></span><span class="line"><span class="comment">%prediction of the magnetic vector</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,121" id="srcline121">121</a></span><span class="line"><span class="keyword">if</span> (<span class="mxinfo" id="T1:U217"><span class="var type1" id="S7T13U357">approx_prediction</span>==<span class="mxinfo" id="T6:U219">1</span></span>)</span></span>
<span class="srcline"><span class="lineno"><a href="1,122" id="srcline122">122</a></span><span class="line">    <span class="comment">%e^(Odt)=I+dt*O+dt^2/2!O^2</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,123" id="srcline123">123</a></span><span class="line">    <span class="comment">% so we do a first order approximation of the exponential map</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,124" id="srcline124">124</a></span><span class="line">    <span class="mxinfo" id="T10:U220"><span class="var type1" id="S50T10U361">muk</span> =<span class="mxinfo" id="T10:U222">(<span class="mxinfo" id="T2:U223"><span class="mxinfo" id="T2:U224"><span class="var type1" id="S47T2U366">O</span>*<span class="var type1" id="S10T5U367">dt</span></span>+<span class="mxinfo" id="T2:U227">single(<span class="mxinfo" id="T21:U228">eye(3)</span>)</span></span>)*<span class="mxinfo" id="T10:U229">[<span class="var type1" id="S41T5U375">mux</span>;<span class="var type1" id="S42T5U377">muy</span>;<span class="var type1" id="S43T5U379">muz</span>]</span></span></span>;</span></span>
<span class="srcline"><span class="lineno"><a href="1,125" id="srcline125">125</a></span><span class="line"><span class="keyword">else</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,126" id="srcline126">126</a></span><span class="line">     <span class="mxinfo" id="T10:U233"><span class="var type1" id="S50T10U383">muk</span> =<span class="mxinfo" id="T10:U235">(<span class="mxinfo" id="T2:U236"><span class="mxinfo" id="T2:U237"><span class="mxinfo" id="T2:U238">single(<span class="mxinfo" id="T21:U239">eye(3)</span>)</span>+<span class="mxinfo" id="T2:U240"><span class="var type1" id="S47T2U394">O</span>*<span class="var type1" id="S10T5U395">dt</span></span></span>+<span class="mxinfo" id="T2:U243"><span class="mxinfo" id="T5:U244"><span class="mxinfo" id="T5:U245"><span class="var type1" id="S10T5U399">dt</span>^2</span>/<span class="mxinfo" id="T6:U247">2</span></span>*<span class="mxinfo" id="T2:U248"><span class="var type1" id="S47T2U403">O</span>^2</span></span></span>)*<span class="mxinfo" id="T10:U250">[<span class="var type1" id="S41T5U407">mux</span>;<span class="var type1" id="S42T5U409">muy</span>;<span class="var type1" id="S43T5U411">muz</span>]</span></span></span>;</span></span>
<span class="srcline"><span class="lineno"><a href="1,127" id="srcline127">127</a></span><span class="line">    <span class="comment">%muk =expm2(O*dt)*[mux;muy;muz]; not working because use double</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,128" id="srcline128">128</a></span><span class="line">    <span class="comment">%precision</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,129" id="srcline129">129</a></span><span class="line"><span class="keyword">end</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,130" id="srcline130">130</a></span><span class="line"></span></span>
<span class="srcline"><span class="lineno"><a href="1,131" id="srcline131">131</a></span><span class="line"><span class="mxinfo" id="T12:U254"><span class="var type1" id="S51T12U414">x_apr</span>=<span class="mxinfo" id="T12:U256">[<span class="var type1" id="S46T10U417">wk</span>;<span class="var type1" id="S44T10U419">wak</span>;<span class="var type1" id="S48T10U421">zek</span>;<span class="var type1" id="S50T10U423">muk</span>]</span></span>;</span></span>
<span class="srcline"><span class="lineno"><a href="1,132" id="srcline132">132</a></span><span class="line"></span></span>
<span class="srcline"><span class="lineno"><a href="1,133" id="srcline133">133</a></span><span class="line"><span class="comment">% compute the apriori error covariance estimate from the previous</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,134" id="srcline134">134</a></span><span class="line"><span class="comment">%aposteriori estimate</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,135" id="srcline135">135</a></span><span class="line"></span></span>
<span class="srcline"><span class="lineno"><a href="1,136" id="srcline136">136</a></span><span class="line"><span class="mxinfo" id="T2:U261"><span class="var type1" id="S52T2U426">EZ</span>=<span class="mxinfo" id="T2:U263"><span class="mxinfo" id="T2:U264">[<span class="mxinfo" id="T39:U265"><span class="mxinfo" id="T6:U266">0</span>,<span class="var type1" id="S40T5U431">zez</span>,<span class="mxinfo" id="T5:U268">-<span class="var type1" id="S39T5U433">zey</span></span></span>;</span></span></span></span></span>
<span class="srcline"><span class="lineno"><a href="1,137" id="srcline137">137</a></span><span class="line"><span class="mxinfo" id="T2:U261"><span class="mxinfo" id="T2:U263"><span class="mxinfo" id="T2:U264">    <span class="mxinfo" id="T39:U270"><span class="mxinfo" id="T5:U271">-<span class="var type1" id="S40T5U436">zez</span></span>,<span class="mxinfo" id="T6:U273">0</span>,<span class="var type1" id="S38T5U438">zex</span></span>;</span></span></span></span></span>
<span class="srcline"><span class="lineno"><a href="1,138" id="srcline138">138</a></span><span class="line"><span class="mxinfo" id="T2:U261"><span class="mxinfo" id="T2:U263"><span class="mxinfo" id="T2:U264">    <span class="mxinfo" id="T39:U275"><span class="var type1" id="S39T5U440">zey</span>,<span class="mxinfo" id="T5:U277">-<span class="var type1" id="S38T5U442">zex</span></span>,<span class="mxinfo" id="T6:U279">0</span></span>]</span>'</span></span>;</span></span>
<span class="srcline"><span class="lineno"><a href="1,139" id="srcline139">139</a></span><span class="line"><span class="mxinfo" id="T2:U280"><span class="var type1" id="S53T2U446">MA</span>=<span class="mxinfo" id="T2:U282"><span class="mxinfo" id="T2:U283">[<span class="mxinfo" id="T39:U284"><span class="mxinfo" id="T6:U285">0</span>,<span class="var type1" id="S43T5U451">muz</span>,<span class="mxinfo" id="T5:U287">-<span class="var type1" id="S42T5U453">muy</span></span></span>;</span></span></span></span></span>
<span class="srcline"><span class="lineno"><a href="1,140" id="srcline140">140</a></span><span class="line"><span class="mxinfo" id="T2:U280"><span class="mxinfo" id="T2:U282"><span class="mxinfo" id="T2:U283">    <span class="mxinfo" id="T39:U289"><span class="mxinfo" id="T5:U290">-<span class="var type1" id="S43T5U456">muz</span></span>,<span class="mxinfo" id="T6:U292">0</span>,<span class="var type1" id="S41T5U458">mux</span></span>;</span></span></span></span></span>
<span class="srcline"><span class="lineno"><a href="1,141" id="srcline141">141</a></span><span class="line"><span class="mxinfo" id="T2:U280"><span class="mxinfo" id="T2:U282"><span class="mxinfo" id="T2:U283">    <span class="mxinfo" id="T39:U294"><span class="var type1" id="S42T5U460">muy</span>,<span class="mxinfo" id="T5:U296">-<span class="var type1" id="S41T5U462">mux</span></span>,<span class="mxinfo" id="T6:U298">0</span></span>]</span>'</span></span>;</span></span>
<span class="srcline"><span class="lineno"><a href="1,142" id="srcline142">142</a></span><span class="line"></span></span>
<span class="srcline"><span class="lineno"><a href="1,143" id="srcline143">143</a></span><span class="line"><span class="mxinfo" id="T2:U299"><span class="var type1" id="S54T2U466">E</span>=<span class="mxinfo" id="T2:U301">single(eye(3))</span></span>;</span></span>
<span class="srcline"><span class="lineno"><a href="1,144" id="srcline144">144</a></span><span class="line"><span class="mxinfo" id="T2:U302"><span class="var type1" id="S55T2U474">Z</span>=<span class="mxinfo" id="T2:U304">single(zeros(3))</span></span>;</span></span>
<span class="srcline"><span class="lineno"><a href="1,145" id="srcline145">145</a></span><span class="line"></span></span>
<span class="srcline"><span class="lineno"><a href="1,146" id="srcline146">146</a></span><span class="line"><span class="mxinfo" id="T24:U305"><span class="var type1" id="S56T24U482">A_lin</span>=<span class="mxinfo" id="T24:U307">[ <span class="mxinfo" id="T37:U308"><span class="var type1" id="S55T2U485">Z</span>,  <span class="var type1" id="S54T2U486">E</span>,  <span class="var type1" id="S55T2U487">Z</span>,  <span class="var type1" id="S55T2U488">Z</span></span></span></span></span></span>
<span class="srcline"><span class="lineno"><a href="1,147" id="srcline147">147</a></span><span class="line"><span class="mxinfo" id="T24:U305"><span class="mxinfo" id="T24:U307">    <span class="mxinfo" id="T37:U313"><span class="var type1" id="S55T2U490">Z</span>,  <span class="var type1" id="S55T2U491">Z</span>,  <span class="var type1" id="S55T2U492">Z</span>,  <span class="var type1" id="S55T2U493">Z</span></span></span></span></span></span>
<span class="srcline"><span class="lineno"><a href="1,148" id="srcline148">148</a></span><span class="line"><span class="mxinfo" id="T24:U305"><span class="mxinfo" id="T24:U307">    <span class="mxinfo" id="T37:U318"><span class="var type1" id="S52T2U495">EZ</span>, <span class="var type1" id="S55T2U496">Z</span>,  <span class="var type1" id="S47T2U497">O</span>,  <span class="var type1" id="S55T2U498">Z</span></span></span></span></span></span>
<span class="srcline"><span class="lineno"><a href="1,149" id="srcline149">149</a></span><span class="line"><span class="mxinfo" id="T24:U305"><span class="mxinfo" id="T24:U307">    <span class="mxinfo" id="T37:U323"><span class="var type1" id="S53T2U500">MA</span>, <span class="var type1" id="S55T2U501">Z</span>,  <span class="var type1" id="S55T2U502">Z</span>,  <span class="var type1" id="S47T2U503">O</span></span>]</span></span>;</span></span>
<span class="srcline"><span class="lineno"><a href="1,150" id="srcline150">150</a></span><span class="line"></span></span>
<span class="srcline"><span class="lineno"><a href="1,151" id="srcline151">151</a></span><span class="line"><span class="mxinfo" id="T24:U328"><span class="var type1" id="S56T24U506">A_lin</span>=<span class="mxinfo" id="T24:U330"><span class="mxinfo" id="T11:U331">eye(12)</span>+<span class="mxinfo" id="T24:U332"><span class="var type1" id="S56T24U512">A_lin</span>*<span class="var type1" id="S10T5U513">dt</span></span></span></span>;</span></span>
<span class="srcline"><span class="lineno"><a href="1,152" id="srcline152">152</a></span><span class="line"></span></span>
<span class="srcline"><span class="lineno"><a href="1,153" id="srcline153">153</a></span><span class="line"><span class="comment">%process covariance matrix</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,154" id="srcline154">154</a></span><span class="line"></span></span>
<span class="srcline"><span class="lineno"><a href="1,155" id="srcline155">155</a></span><span class="line"><span class="keyword">persistent</span> <span class="var type1" id="S57T24U515">Q</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,156" id="srcline156">156</a></span><span class="line"><span class="keyword">if</span> (<span class="mxinfo" id="T1:U336"><span class="mxinfo" id="T1:U337">isempty(<span class="var type0" id="S57T0U521">Q</span>)</span></span>)</span></span>
<span class="srcline"><span class="lineno"><a href="1,157" id="srcline157">157</a></span><span class="line">    <span class="mxinfo" id="T24:U338"><span class="var type1" id="S57T24U524">Q</span>=<span class="mxinfo" id="T24:U340">diag(<span class="mxinfo" id="T25:U341">[ <span class="var type1" id="S12T5U529">q_rotSpeed</span>,<span class="var type1" id="S12T5U530">q_rotSpeed</span>,<span class="var type1" id="S12T5U531">q_rotSpeed</span>,<span class="keyword">...</span></span></span></span></span></span>
<span class="srcline"><span class="lineno"><a href="1,158" id="srcline158">158</a></span><span class="line"><span class="mxinfo" id="T24:U338"><span class="mxinfo" id="T24:U340"><span class="mxinfo" id="T25:U341">        <span class="var type1" id="S13T5U532">q_rotAcc</span>,<span class="var type1" id="S13T5U533">q_rotAcc</span>,<span class="var type1" id="S13T5U534">q_rotAcc</span>,<span class="keyword">...</span></span></span></span></span></span>
<span class="srcline"><span class="lineno"><a href="1,159" id="srcline159">159</a></span><span class="line"><span class="mxinfo" id="T24:U338"><span class="mxinfo" id="T24:U340"><span class="mxinfo" id="T25:U341">        <span class="var type1" id="S14T5U535">q_acc</span>,<span class="var type1" id="S14T5U536">q_acc</span>,<span class="var type1" id="S14T5U537">q_acc</span>,<span class="keyword">...</span></span></span></span></span></span>
<span class="srcline"><span class="lineno"><a href="1,160" id="srcline160">160</a></span><span class="line"><span class="mxinfo" id="T24:U338"><span class="mxinfo" id="T24:U340"><span class="mxinfo" id="T25:U341">        <span class="var type1" id="S15T5U538">q_mag</span>,<span class="var type1" id="S15T5U539">q_mag</span>,<span class="var type1" id="S15T5U540">q_mag</span>]</span>)</span></span>;</span></span>
<span class="srcline"><span class="lineno"><a href="1,161" id="srcline161">161</a></span><span class="line"><span class="keyword">end</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,162" id="srcline162">162</a></span><span class="line"></span></span>
<span class="srcline"><span class="lineno"><a href="1,163" id="srcline163">163</a></span><span class="line"><span class="mxinfo" id="T24:U354"><span class="var type1" id="S59T24U543">P_apr</span>=<span class="mxinfo" id="T24:U356"><span class="mxinfo" id="T24:U357"><span class="mxinfo" id="T24:U358"><span class="var type1" id="S56T24U547">A_lin</span>*<span class="var type1" id="S29T24U548">P_apo</span></span>*<span class="mxinfo" id="T24:U361"><span class="var type1" id="S56T24U550">A_lin</span>'</span></span>+<span class="var type1" id="S57T24U551">Q</span></span></span>;</span></span>
<span class="srcline"><span class="lineno"><a href="1,164" id="srcline164">164</a></span><span class="line"></span></span>
<span class="srcline"><span class="lineno"><a href="1,165" id="srcline165">165</a></span><span class="line"></span></span>
<span class="srcline"><span class="lineno"><a href="1,166" id="srcline166">166</a></span><span class="line"><span class="comment">%% update</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,167" id="srcline167">167</a></span><span class="line"><span class="keyword">if</span> <span class="mxinfo" id="T1:U364"><span class="mxinfo" id="T13:U365"><span class="var type1" id="S9T26U558">zFlag</span>(<span class="mxinfo" id="T6:U367">1</span>)</span>==<span class="mxinfo" id="T6:U368">1</span></span>&amp;&amp;<span class="mxinfo" id="T1:U369"><span class="mxinfo" id="T13:U370"><span class="var type1" id="S9T26U563">zFlag</span>(<span class="mxinfo" id="T6:U372">2</span>)</span>==<span class="mxinfo" id="T6:U373">1</span></span>&amp;&amp;<span class="mxinfo" id="T1:U374"><span class="mxinfo" id="T13:U375"><span class="var type1" id="S9T26U568">zFlag</span>(<span class="mxinfo" id="T6:U377">3</span>)</span>==<span class="mxinfo" id="T6:U378">1</span></span></span></span>
<span class="srcline"><span class="lineno"><a href="1,168" id="srcline168">168</a></span><span class="line">    </span></span>
<span class="srcline"><span class="lineno"><a href="1,169" id="srcline169">169</a></span><span class="line"><span class="comment">%     R=[r_gyro,0,0,0,0,0,0,0,0;</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,170" id="srcline170">170</a></span><span class="line"><span class="comment">%         0,r_gyro,0,0,0,0,0,0,0;</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,171" id="srcline171">171</a></span><span class="line"><span class="comment">%         0,0,r_gyro,0,0,0,0,0,0;</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,172" id="srcline172">172</a></span><span class="line"><span class="comment">%         0,0,0,r_accel,0,0,0,0,0;</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,173" id="srcline173">173</a></span><span class="line"><span class="comment">%         0,0,0,0,r_accel,0,0,0,0;</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,174" id="srcline174">174</a></span><span class="line"><span class="comment">%         0,0,0,0,0,r_accel,0,0,0;</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,175" id="srcline175">175</a></span><span class="line"><span class="comment">%         0,0,0,0,0,0,r_mag,0,0;</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,176" id="srcline176">176</a></span><span class="line"><span class="comment">%         0,0,0,0,0,0,0,r_mag,0;</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,177" id="srcline177">177</a></span><span class="line"><span class="comment">%         0,0,0,0,0,0,0,0,r_mag];</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,178" id="srcline178">178</a></span><span class="line">     <span class="mxinfo" id="T31:U379"><span class="var type1" id="S60T31U573">R_v</span>=<span class="mxinfo" id="T31:U381">[<span class="var type1" id="S16T5U576">r_gyro</span>,<span class="var type1" id="S16T5U577">r_gyro</span>,<span class="var type1" id="S16T5U578">r_gyro</span>,<span class="var type1" id="S17T5U579">r_accel</span>,<span class="var type1" id="S17T5U580">r_accel</span>,<span class="var type1" id="S17T5U581">r_accel</span>,<span class="var type1" id="S18T5U582">r_mag</span>,<span class="var type1" id="S18T5U583">r_mag</span>,<span class="var type1" id="S18T5U584">r_mag</span>]</span></span>;</span></span>
<span class="srcline"><span class="lineno"><a href="1,179" id="srcline179">179</a></span><span class="line">    <span class="comment">%observation matrix</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,180" id="srcline180">180</a></span><span class="line">    <span class="comment">%[zw;ze;zmk];</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,181" id="srcline181">181</a></span><span class="line">    <span class="mxinfo" id="T28:U391"><span class="var type1" id="S61T28U587">H_k</span>=<span class="mxinfo" id="T28:U393">[  <span class="var type1" id="S54T2U590">E</span>,     <span class="var type1" id="S55T2U591">Z</span>,      <span class="var type1" id="S55T2U592">Z</span>,    <span class="var type1" id="S55T2U593">Z</span>;</span></span></span></span>
<span class="srcline"><span class="lineno"><a href="1,182" id="srcline182">182</a></span><span class="line"><span class="mxinfo" id="T28:U391"><span class="mxinfo" id="T28:U393">        <span class="var type1" id="S55T2U595">Z</span>,     <span class="var type1" id="S55T2U596">Z</span>,      <span class="var type1" id="S54T2U597">E</span>,    <span class="var type1" id="S55T2U598">Z</span>;</span></span></span></span>
<span class="srcline"><span class="lineno"><a href="1,183" id="srcline183">183</a></span><span class="line"><span class="mxinfo" id="T28:U391"><span class="mxinfo" id="T28:U393">        <span class="var type1" id="S55T2U600">Z</span>,     <span class="var type1" id="S55T2U601">Z</span>,      <span class="var type1" id="S55T2U602">Z</span>,    <span class="var type1" id="S54T2U603">E</span>]</span></span>;</span></span>
<span class="srcline"><span class="lineno"><a href="1,184" id="srcline184">184</a></span><span class="line">    </span></span>
<span class="srcline"><span class="lineno"><a href="1,185" id="srcline185">185</a></span><span class="line">    <span class="mxinfo" id="T27:U406"><span class="var type1" id="S62T27U606">y_k</span>=<span class="mxinfo" id="T27:U408"><span class="mxinfo" id="T27:U409"><span class="var type1" id="S11T27U609">z</span>(<span class="mxinfo" id="T48:U411"><span class="mxinfo" id="T6:U412">1</span>:<span class="mxinfo" id="T6:U413">9</span></span>)</span>-<span class="mxinfo" id="T27:U414"><span class="mxinfo" id="T28:U415"><span class="var type1" id="S61T28U614">H_k</span></span>*<span class="var type1" id="S51T12U615">x_apr</span></span></span></span>;</span></span>
<span class="srcline"><span class="lineno"><a href="1,186" id="srcline186">186</a></span><span class="line">    </span></span>
<span class="srcline"><span class="lineno"><a href="1,187" id="srcline187">187</a></span><span class="line">    </span></span>
<span class="srcline"><span class="lineno"><a href="1,188" id="srcline188">188</a></span><span class="line">    <span class="comment">%S_k=H_k*P_apr*H_k'+R;</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,189" id="srcline189">189</a></span><span class="line">     <span class="mxinfo" id="T30:U418"><span class="var type1" id="S63T30U618">S_k</span>=<span class="mxinfo" id="T30:U420"><span class="mxinfo" id="T28:U421"><span class="mxinfo" id="T28:U422"><span class="var type1" id="S61T28U621">H_k</span></span>*<span class="var type1" id="S59T24U622">P_apr</span></span>*<span class="mxinfo" id="T29:U425"><span class="var type1" id="S61T28U624">H_k</span>'</span></span></span>;</span></span>
<span class="srcline"><span class="lineno"><a href="1,190" id="srcline190">190</a></span><span class="line">     <span class="mxinfo" id="T31:U427"><span class="mxinfo" id="T31:U428"><span class="var type1" id="S63T30U628">S_k</span>(<span class="mxinfo" id="T48:U430"><span class="mxinfo" id="T6:U431">1</span>:<span class="mxinfo" id="T6:U432">9+1</span>:<span class="mxinfo" id="T6:U433">end</span></span>)</span> = <span class="mxinfo" id="T31:U434"><span class="mxinfo" id="T31:U435"><span class="var type1" id="S63T30U639">S_k</span>(<span class="mxinfo" id="T48:U437"><span class="mxinfo" id="T6:U438">1</span>:<span class="mxinfo" id="T6:U439">9+1</span>:<span class="mxinfo" id="T6:U440">end</span></span>)</span> + <span class="var type1" id="S60T31U648">R_v</span></span></span>;</span></span>
<span class="srcline"><span class="lineno"><a href="1,191" id="srcline191">191</a></span><span class="line">    <span class="mxinfo" id="T29:U442"><span class="var type1" id="S65T29U651">K_k</span>=(<span class="mxinfo" id="T29:U444"><span class="mxinfo" id="T29:U445"><span class="var type1" id="S59T24U655">P_apr</span>*<span class="mxinfo" id="T29:U447"><span class="var type1" id="S61T28U657">H_k</span>'</span></span>/(<span class="var type1" id="S63T30U659">S_k</span>)</span>)</span>;</span></span>
<span class="srcline"><span class="lineno"><a href="1,192" id="srcline192">192</a></span><span class="line">    </span></span>
<span class="srcline"><span class="lineno"><a href="1,193" id="srcline193">193</a></span><span class="line">    </span></span>
<span class="srcline"><span class="lineno"><a href="1,194" id="srcline194">194</a></span><span class="line">    <span class="mxinfo" id="T12:U450"><span class="var type1" id="S24T12U662">x_apo</span>=<span class="mxinfo" id="T12:U452"><span class="var type1" id="S51T12U664">x_apr</span>+<span class="mxinfo" id="T12:U454"><span class="var type1" id="S65T29U666">K_k</span>*<span class="var type1" id="S62T27U667">y_k</span></span></span></span>;</span></span>
<span class="srcline"><span class="lineno"><a href="1,195" id="srcline195">195</a></span><span class="line">    <span class="mxinfo" id="T24:U457"><span class="var type1" id="S29T24U670">P_apo</span>=<span class="mxinfo" id="T24:U459">(<span class="mxinfo" id="T24:U460"><span class="mxinfo" id="T11:U461">eye(12)</span>-<span class="mxinfo" id="T24:U462"><span class="var type1" id="S65T29U678">K_k</span>*<span class="mxinfo" id="T28:U464"><span class="var type1" id="S61T28U679">H_k</span></span></span></span>)*<span class="var type1" id="S59T24U680">P_apr</span></span></span>;</span></span>
<span class="srcline"><span class="lineno"><a href="1,196" id="srcline196">196</a></span><span class="line"><span class="keyword">else</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,197" id="srcline197">197</a></span><span class="line">    <span class="keyword">if</span> <span class="mxinfo" id="T1:U467"><span class="mxinfo" id="T13:U468"><span class="var type1" id="S9T26U688">zFlag</span>(<span class="mxinfo" id="T6:U470">1</span>)</span>==<span class="mxinfo" id="T6:U471">1</span></span>&amp;&amp;<span class="mxinfo" id="T1:U472"><span class="mxinfo" id="T13:U473"><span class="var type1" id="S9T26U693">zFlag</span>(<span class="mxinfo" id="T6:U475">2</span>)</span>==<span class="mxinfo" id="T6:U476">0</span></span>&amp;&amp;<span class="mxinfo" id="T1:U477"><span class="mxinfo" id="T13:U478"><span class="var type1" id="S9T26U698">zFlag</span>(<span class="mxinfo" id="T6:U480">3</span>)</span>==<span class="mxinfo" id="T6:U481">0</span></span></span></span>
<span class="srcline"><span class="lineno"><a href="1,198" id="srcline198">198</a></span><span class="line">        </span></span>
<span class="srcline"><span class="lineno"><a href="1,199" id="srcline199">199</a></span><span class="line">        <span class="mxinfo" id="T2:U482"><span class="var type1" id="S66T2U703">R</span>=<span class="mxinfo" id="T2:U484">[<span class="mxinfo" id="T39:U485"><span class="var type1" id="S16T5U706">r_gyro</span>,<span class="mxinfo" id="T6:U487">0</span>,<span class="mxinfo" id="T6:U488">0</span></span>;</span></span></span></span>
<span class="srcline"><span class="lineno"><a href="1,200" id="srcline200">200</a></span><span class="line"><span class="mxinfo" id="T2:U482"><span class="mxinfo" id="T2:U484">            <span class="mxinfo" id="T39:U489"><span class="mxinfo" id="T6:U490">0</span>,<span class="var type1" id="S16T5U711">r_gyro</span>,<span class="mxinfo" id="T6:U492">0</span></span>;</span></span></span></span>
<span class="srcline"><span class="lineno"><a href="1,201" id="srcline201">201</a></span><span class="line"><span class="mxinfo" id="T2:U482"><span class="mxinfo" id="T2:U484">            <span class="mxinfo" id="T39:U493"><span class="mxinfo" id="T6:U494">0</span>,<span class="mxinfo" id="T6:U495">0</span>,<span class="var type1" id="S16T5U716">r_gyro</span></span>]</span></span>;</span></span>
<span class="srcline"><span class="lineno"><a href="1,202" id="srcline202">202</a></span><span class="line">        <span class="mxinfo" id="T39:U497"><span class="var type1" id="S60T39U719">R_v</span>=<span class="mxinfo" id="T39:U499">[<span class="var type1" id="S16T5U722">r_gyro</span>,<span class="var type1" id="S16T5U723">r_gyro</span>,<span class="var type1" id="S16T5U724">r_gyro</span>]</span></span>;</span></span>
<span class="srcline"><span class="lineno"><a href="1,203" id="srcline203">203</a></span><span class="line">        <span class="comment">%observation matrix</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,204" id="srcline204">204</a></span><span class="line">        </span></span>
<span class="srcline"><span class="lineno"><a href="1,205" id="srcline205">205</a></span><span class="line">        <span class="mxinfo" id="T37:U503"><span class="var type1" id="S61T37U727">H_k</span>=<span class="mxinfo" id="T37:U505">[  <span class="var type1" id="S54T2U730">E</span>,     <span class="var type1" id="S55T2U731">Z</span>,      <span class="var type1" id="S55T2U732">Z</span>,    <span class="var type1" id="S55T2U733">Z</span>]</span></span>;</span></span>
<span class="srcline"><span class="lineno"><a href="1,206" id="srcline206">206</a></span><span class="line">        </span></span>
<span class="srcline"><span class="lineno"><a href="1,207" id="srcline207">207</a></span><span class="line">        <span class="mxinfo" id="T10:U510"><span class="var type1" id="S62T10U736">y_k</span>=<span class="mxinfo" id="T10:U512"><span class="mxinfo" id="T10:U513"><span class="var type1" id="S11T27U739">z</span>(<span class="mxinfo" id="T49:U515"><span class="mxinfo" id="T6:U516">1</span>:<span class="mxinfo" id="T6:U517">3</span></span>)</span>-<span class="mxinfo" id="T10:U518"><span class="mxinfo" id="T37:U519"><span class="var type1" id="S61T37U745">H_k</span>(1:3,1:12)</span>*<span class="var type1" id="S51T12U752">x_apr</span></span></span></span>;</span></span>
<span class="srcline"><span class="lineno"><a href="1,208" id="srcline208">208</a></span><span class="line">        </span></span>
<span class="srcline"><span class="lineno"><a href="1,209" id="srcline209">209</a></span><span class="line">       <span class="comment">% S_k=H_k(1:3,1:12)*P_apr*H_k(1:3,1:12)'+R(1:3,1:3);</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,210" id="srcline210">210</a></span><span class="line">        <span class="mxinfo" id="T2:U522"><span class="var type1" id="S63T2U755">S_k</span>=<span class="mxinfo" id="T2:U524"><span class="mxinfo" id="T37:U525"><span class="mxinfo" id="T37:U526"><span class="var type1" id="S61T37U759">H_k</span>(1:3,1:12)</span>*<span class="var type1" id="S59T24U766">P_apr</span></span>*<span class="mxinfo" id="T38:U529"><span class="var type1" id="S61T37U769">H_k</span>(1:3,1:12)'</span></span></span>;</span></span>
<span class="srcline"><span class="lineno"><a href="1,211" id="srcline211">211</a></span><span class="line">        <span class="mxinfo" id="T39:U531"><span class="mxinfo" id="T39:U532"><span class="var type1" id="S63T2U779">S_k</span>(<span class="mxinfo" id="T49:U534"><span class="mxinfo" id="T6:U535">1</span>:<span class="mxinfo" id="T6:U536">3+1</span>:<span class="mxinfo" id="T6:U537">end</span></span>)</span> = <span class="mxinfo" id="T39:U538"><span class="mxinfo" id="T39:U539"><span class="var type1" id="S63T2U790">S_k</span>(<span class="mxinfo" id="T49:U541"><span class="mxinfo" id="T6:U542">1</span>:<span class="mxinfo" id="T6:U543">3+1</span>:<span class="mxinfo" id="T6:U544">end</span></span>)</span> + <span class="var type1" id="S60T39U799">R_v</span></span></span>;</span></span>
<span class="srcline"><span class="lineno"><a href="1,212" id="srcline212">212</a></span><span class="line">        <span class="mxinfo" id="T38:U546"><span class="var type1" id="S65T38U802">K_k</span>=(<span class="mxinfo" id="T38:U548"><span class="mxinfo" id="T38:U549"><span class="var type1" id="S59T24U806">P_apr</span>*<span class="mxinfo" id="T38:U551"><span class="var type1" id="S61T37U809">H_k</span>(1:3,1:12)'</span></span>/(<span class="var type1" id="S63T2U817">S_k</span>)</span>)</span>;</span></span>
<span class="srcline"><span class="lineno"><a href="1,213" id="srcline213">213</a></span><span class="line">        </span></span>
<span class="srcline"><span class="lineno"><a href="1,214" id="srcline214">214</a></span><span class="line">        </span></span>
<span class="srcline"><span class="lineno"><a href="1,215" id="srcline215">215</a></span><span class="line">        <span class="mxinfo" id="T12:U554"><span class="var type1" id="S24T12U820">x_apo</span>=<span class="mxinfo" id="T12:U556"><span class="var type1" id="S51T12U822">x_apr</span>+<span class="mxinfo" id="T12:U558"><span class="var type1" id="S65T38U824">K_k</span>*<span class="var type1" id="S62T10U825">y_k</span></span></span></span>;</span></span>
<span class="srcline"><span class="lineno"><a href="1,216" id="srcline216">216</a></span><span class="line">        <span class="mxinfo" id="T24:U561"><span class="var type1" id="S29T24U828">P_apo</span>=<span class="mxinfo" id="T24:U563">(<span class="mxinfo" id="T24:U564"><span class="mxinfo" id="T11:U565">eye(12)</span>-<span class="mxinfo" id="T24:U566"><span class="var type1" id="S65T38U836">K_k</span>*<span class="mxinfo" id="T37:U568"><span class="var type1" id="S61T37U838">H_k</span>(1:3,1:12)</span></span></span>)*<span class="var type1" id="S59T24U845">P_apr</span></span></span>;</span></span>
<span class="srcline"><span class="lineno"><a href="1,217" id="srcline217">217</a></span><span class="line">    <span class="keyword">else</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,218" id="srcline218">218</a></span><span class="line">        <span class="keyword">if</span>  <span class="mxinfo" id="T1:U571"><span class="mxinfo" id="T13:U572"><span class="var type1" id="S9T26U853">zFlag</span>(<span class="mxinfo" id="T6:U574">1</span>)</span>==<span class="mxinfo" id="T6:U575">1</span></span>&amp;&amp;<span class="mxinfo" id="T1:U576"><span class="mxinfo" id="T13:U577"><span class="var type1" id="S9T26U858">zFlag</span>(<span class="mxinfo" id="T6:U579">2</span>)</span>==<span class="mxinfo" id="T6:U580">1</span></span>&amp;&amp;<span class="mxinfo" id="T1:U581"><span class="mxinfo" id="T13:U582"><span class="var type1" id="S9T26U863">zFlag</span>(<span class="mxinfo" id="T6:U584">3</span>)</span>==<span class="mxinfo" id="T6:U585">0</span></span></span></span>
<span class="srcline"><span class="lineno"><a href="1,219" id="srcline219">219</a></span><span class="line">            </span></span>
<span class="srcline"><span class="lineno"><a href="1,220" id="srcline220">220</a></span><span class="line"><span class="comment">%             R=[r_gyro,0,0,0,0,0;</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,221" id="srcline221">221</a></span><span class="line"><span class="comment">%                 0,r_gyro,0,0,0,0;</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,222" id="srcline222">222</a></span><span class="line"><span class="comment">%                 0,0,r_gyro,0,0,0;</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,223" id="srcline223">223</a></span><span class="line"><span class="comment">%                 0,0,0,r_accel,0,0;</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,224" id="srcline224">224</a></span><span class="line"><span class="comment">%                 0,0,0,0,r_accel,0;</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,225" id="srcline225">225</a></span><span class="line"><span class="comment">%                 0,0,0,0,0,r_accel];</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,226" id="srcline226">226</a></span><span class="line">            </span></span>
<span class="srcline"><span class="lineno"><a href="1,227" id="srcline227">227</a></span><span class="line">            <span class="mxinfo" id="T44:U586"><span class="var type1" id="S60T44U868">R_v</span>=<span class="mxinfo" id="T44:U588">[<span class="var type1" id="S16T5U871">r_gyro</span>,<span class="var type1" id="S16T5U872">r_gyro</span>,<span class="var type1" id="S16T5U873">r_gyro</span>,<span class="var type1" id="S17T5U874">r_accel</span>,<span class="var type1" id="S17T5U875">r_accel</span>,<span class="var type1" id="S17T5U876">r_accel</span>]</span></span>;</span></span>
<span class="srcline"><span class="lineno"><a href="1,228" id="srcline228">228</a></span><span class="line">            <span class="comment">%observation matrix</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,229" id="srcline229">229</a></span><span class="line">            </span></span>
<span class="srcline"><span class="lineno"><a href="1,230" id="srcline230">230</a></span><span class="line">            <span class="mxinfo" id="T40:U595"><span class="var type1" id="S61T40U879">H_k</span>=<span class="mxinfo" id="T40:U597">[  <span class="var type1" id="S54T2U882">E</span>,     <span class="var type1" id="S55T2U883">Z</span>,      <span class="var type1" id="S55T2U884">Z</span>,    <span class="var type1" id="S55T2U885">Z</span>;</span></span></span></span>
<span class="srcline"><span class="lineno"><a href="1,231" id="srcline231">231</a></span><span class="line"><span class="mxinfo" id="T40:U595"><span class="mxinfo" id="T40:U597">                <span class="var type1" id="S55T2U887">Z</span>,     <span class="var type1" id="S55T2U888">Z</span>,      <span class="var type1" id="S54T2U889">E</span>,    <span class="var type1" id="S55T2U890">Z</span>]</span></span>;</span></span>
<span class="srcline"><span class="lineno"><a href="1,232" id="srcline232">232</a></span><span class="line">            </span></span>
<span class="srcline"><span class="lineno"><a href="1,233" id="srcline233">233</a></span><span class="line">            <span class="mxinfo" id="T41:U606"><span class="var type1" id="S62T41U893">y_k</span>=<span class="mxinfo" id="T41:U608"><span class="mxinfo" id="T41:U609"><span class="var type1" id="S11T27U896">z</span>(<span class="mxinfo" id="T50:U611"><span class="mxinfo" id="T6:U612">1</span>:<span class="mxinfo" id="T6:U613">6</span></span>)</span>-<span class="mxinfo" id="T41:U614"><span class="mxinfo" id="T40:U615"><span class="var type1" id="S61T40U902">H_k</span>(1:6,1:12)</span>*<span class="var type1" id="S51T12U909">x_apr</span></span></span></span>;</span></span>
<span class="srcline"><span class="lineno"><a href="1,234" id="srcline234">234</a></span><span class="line">            </span></span>
<span class="srcline"><span class="lineno"><a href="1,235" id="srcline235">235</a></span><span class="line">           <span class="comment">% S_k=H_k(1:6,1:12)*P_apr*H_k(1:6,1:12)'+R(1:6,1:6);</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,236" id="srcline236">236</a></span><span class="line">            <span class="mxinfo" id="T43:U618"><span class="var type1" id="S63T43U912">S_k</span>=<span class="mxinfo" id="T43:U620"><span class="mxinfo" id="T40:U621"><span class="mxinfo" id="T40:U622"><span class="var type1" id="S61T40U916">H_k</span>(1:6,1:12)</span>*<span class="var type1" id="S59T24U923">P_apr</span></span>*<span class="mxinfo" id="T42:U625"><span class="var type1" id="S61T40U926">H_k</span>(1:6,1:12)'</span></span></span>;</span></span>
<span class="srcline"><span class="lineno"><a href="1,237" id="srcline237">237</a></span><span class="line">            <span class="mxinfo" id="T44:U627"><span class="mxinfo" id="T44:U628"><span class="var type1" id="S63T43U936">S_k</span>(<span class="mxinfo" id="T50:U630"><span class="mxinfo" id="T6:U631">1</span>:<span class="mxinfo" id="T6:U632">6+1</span>:<span class="mxinfo" id="T6:U633">end</span></span>)</span> = <span class="mxinfo" id="T44:U634"><span class="mxinfo" id="T44:U635"><span class="var type1" id="S63T43U947">S_k</span>(<span class="mxinfo" id="T50:U637"><span class="mxinfo" id="T6:U638">1</span>:<span class="mxinfo" id="T6:U639">6+1</span>:<span class="mxinfo" id="T6:U640">end</span></span>)</span> + <span class="var type1" id="S60T44U956">R_v</span></span></span>;</span></span>
<span class="srcline"><span class="lineno"><a href="1,238" id="srcline238">238</a></span><span class="line">            <span class="mxinfo" id="T42:U642"><span class="var type1" id="S65T42U959">K_k</span>=(<span class="mxinfo" id="T42:U644"><span class="mxinfo" id="T42:U645"><span class="var type1" id="S59T24U963">P_apr</span>*<span class="mxinfo" id="T42:U647"><span class="var type1" id="S61T40U966">H_k</span>(1:6,1:12)'</span></span>/(<span class="var type1" id="S63T43U974">S_k</span>)</span>)</span>;</span></span>
<span class="srcline"><span class="lineno"><a href="1,239" id="srcline239">239</a></span><span class="line">            </span></span>
<span class="srcline"><span class="lineno"><a href="1,240" id="srcline240">240</a></span><span class="line">            </span></span>
<span class="srcline"><span class="lineno"><a href="1,241" id="srcline241">241</a></span><span class="line">            <span class="mxinfo" id="T12:U650"><span class="var type1" id="S24T12U977">x_apo</span>=<span class="mxinfo" id="T12:U652"><span class="var type1" id="S51T12U979">x_apr</span>+<span class="mxinfo" id="T12:U654"><span class="var type1" id="S65T42U981">K_k</span>*<span class="var type1" id="S62T41U982">y_k</span></span></span></span>;</span></span>
<span class="srcline"><span class="lineno"><a href="1,242" id="srcline242">242</a></span><span class="line">            <span class="mxinfo" id="T24:U657"><span class="var type1" id="S29T24U985">P_apo</span>=<span class="mxinfo" id="T24:U659">(<span class="mxinfo" id="T24:U660"><span class="mxinfo" id="T11:U661">eye(12)</span>-<span class="mxinfo" id="T24:U662"><span class="var type1" id="S65T42U993">K_k</span>*<span class="mxinfo" id="T40:U664"><span class="var type1" id="S61T40U995">H_k</span>(1:6,1:12)</span></span></span>)*<span class="var type1" id="S59T24U1002">P_apr</span></span></span>;</span></span>
<span class="srcline"><span class="lineno"><a href="1,243" id="srcline243">243</a></span><span class="line">        <span class="keyword">else</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,244" id="srcline244">244</a></span><span class="line">            <span class="keyword">if</span>  <span class="mxinfo" id="T1:U667"><span class="mxinfo" id="T13:U668"><span class="var type1" id="S9T26U1010">zFlag</span>(<span class="mxinfo" id="T6:U670">1</span>)</span>==<span class="mxinfo" id="T6:U671">1</span></span>&amp;&amp;<span class="mxinfo" id="T1:U672"><span class="mxinfo" id="T13:U673"><span class="var type1" id="S9T26U1015">zFlag</span>(<span class="mxinfo" id="T6:U675">2</span>)</span>==<span class="mxinfo" id="T6:U676">0</span></span>&amp;&amp;<span class="mxinfo" id="T1:U677"><span class="mxinfo" id="T13:U678"><span class="var type1" id="S9T26U1020">zFlag</span>(<span class="mxinfo" id="T6:U680">3</span>)</span>==<span class="mxinfo" id="T6:U681">1</span></span></span></span>
<span class="srcline"><span class="lineno"><a href="1,245" id="srcline245">245</a></span><span class="line"><span class="comment">%                 R=[r_gyro,0,0,0,0,0;</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,246" id="srcline246">246</a></span><span class="line"><span class="comment">%                     0,r_gyro,0,0,0,0;</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,247" id="srcline247">247</a></span><span class="line"><span class="comment">%                     0,0,r_gyro,0,0,0;</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,248" id="srcline248">248</a></span><span class="line"><span class="comment">%                     0,0,0,r_mag,0,0;</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,249" id="srcline249">249</a></span><span class="line"><span class="comment">%                     0,0,0,0,r_mag,0;</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,250" id="srcline250">250</a></span><span class="line"><span class="comment">%                     0,0,0,0,0,r_mag];</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,251" id="srcline251">251</a></span><span class="line">                  <span class="mxinfo" id="T44:U682"><span class="var type1" id="S60T44U1025">R_v</span>=<span class="mxinfo" id="T44:U684">[<span class="var type1" id="S16T5U1028">r_gyro</span>,<span class="var type1" id="S16T5U1029">r_gyro</span>,<span class="var type1" id="S16T5U1030">r_gyro</span>,<span class="var type1" id="S18T5U1031">r_mag</span>,<span class="var type1" id="S18T5U1032">r_mag</span>,<span class="var type1" id="S18T5U1033">r_mag</span>]</span></span>;</span></span>
<span class="srcline"><span class="lineno"><a href="1,252" id="srcline252">252</a></span><span class="line">                <span class="comment">%observation matrix</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,253" id="srcline253">253</a></span><span class="line">                </span></span>
<span class="srcline"><span class="lineno"><a href="1,254" id="srcline254">254</a></span><span class="line">                <span class="mxinfo" id="T40:U691"><span class="var type1" id="S61T40U1036">H_k</span>=<span class="mxinfo" id="T40:U693">[  <span class="var type1" id="S54T2U1039">E</span>,     <span class="var type1" id="S55T2U1040">Z</span>,      <span class="var type1" id="S55T2U1041">Z</span>,    <span class="var type1" id="S55T2U1042">Z</span>;</span></span></span></span>
<span class="srcline"><span class="lineno"><a href="1,255" id="srcline255">255</a></span><span class="line"><span class="mxinfo" id="T40:U691"><span class="mxinfo" id="T40:U693">                    <span class="var type1" id="S55T2U1044">Z</span>,     <span class="var type1" id="S55T2U1045">Z</span>,      <span class="var type1" id="S55T2U1046">Z</span>,    <span class="var type1" id="S54T2U1047">E</span>]</span></span>;</span></span>
<span class="srcline"><span class="lineno"><a href="1,256" id="srcline256">256</a></span><span class="line">                </span></span>
<span class="srcline"><span class="lineno"><a href="1,257" id="srcline257">257</a></span><span class="line">                <span class="mxinfo" id="T41:U702"><span class="var type1" id="S62T41U1050">y_k</span>=<span class="mxinfo" id="T41:U704"><span class="mxinfo" id="T41:U705">[<span class="mxinfo" id="T10:U706"><span class="var type1" id="S11T27U1055">z</span>(<span class="mxinfo" id="T49:U708"><span class="mxinfo" id="T6:U709">1</span>:<span class="mxinfo" id="T6:U710">3</span></span>)</span>;<span class="mxinfo" id="T10:U711"><span class="var type1" id="S11T27U1061">z</span>(<span class="mxinfo" id="T49:U713"><span class="mxinfo" id="T6:U714">7</span>:<span class="mxinfo" id="T6:U715">9</span></span>)</span>]</span>-<span class="mxinfo" id="T41:U716"><span class="mxinfo" id="T40:U717"><span class="var type1" id="S61T40U1067">H_k</span>(1:6,1:12)</span>*<span class="var type1" id="S51T12U1074">x_apr</span></span></span></span>;</span></span>
<span class="srcline"><span class="lineno"><a href="1,258" id="srcline258">258</a></span><span class="line">                </span></span>
<span class="srcline"><span class="lineno"><a href="1,259" id="srcline259">259</a></span><span class="line">                <span class="comment">%S_k=H_k(1:6,1:12)*P_apr*H_k(1:6,1:12)'+R(1:6,1:6);</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,260" id="srcline260">260</a></span><span class="line">                <span class="mxinfo" id="T43:U720"><span class="var type1" id="S63T43U1077">S_k</span>=<span class="mxinfo" id="T43:U722"><span class="mxinfo" id="T40:U723"><span class="mxinfo" id="T40:U724"><span class="var type1" id="S61T40U1081">H_k</span>(1:6,1:12)</span>*<span class="var type1" id="S59T24U1088">P_apr</span></span>*<span class="mxinfo" id="T42:U727"><span class="var type1" id="S61T40U1091">H_k</span>(1:6,1:12)'</span></span></span>;</span></span>
<span class="srcline"><span class="lineno"><a href="1,261" id="srcline261">261</a></span><span class="line">                <span class="mxinfo" id="T44:U729"><span class="mxinfo" id="T44:U730"><span class="var type1" id="S63T43U1101">S_k</span>(<span class="mxinfo" id="T50:U732"><span class="mxinfo" id="T6:U733">1</span>:<span class="mxinfo" id="T6:U734">6+1</span>:<span class="mxinfo" id="T6:U735">end</span></span>)</span> = <span class="mxinfo" id="T44:U736"><span class="mxinfo" id="T44:U737"><span class="var type1" id="S63T43U1112">S_k</span>(<span class="mxinfo" id="T50:U739"><span class="mxinfo" id="T6:U740">1</span>:<span class="mxinfo" id="T6:U741">6+1</span>:<span class="mxinfo" id="T6:U742">end</span></span>)</span> + <span class="var type1" id="S60T44U1121">R_v</span></span></span>;</span></span>
<span class="srcline"><span class="lineno"><a href="1,262" id="srcline262">262</a></span><span class="line">                <span class="mxinfo" id="T42:U744"><span class="var type1" id="S65T42U1124">K_k</span>=(<span class="mxinfo" id="T42:U746"><span class="mxinfo" id="T42:U747"><span class="var type1" id="S59T24U1128">P_apr</span>*<span class="mxinfo" id="T42:U749"><span class="var type1" id="S61T40U1131">H_k</span>(1:6,1:12)'</span></span>/(<span class="var type1" id="S63T43U1139">S_k</span>)</span>)</span>;</span></span>
<span class="srcline"><span class="lineno"><a href="1,263" id="srcline263">263</a></span><span class="line">                </span></span>
<span class="srcline"><span class="lineno"><a href="1,264" id="srcline264">264</a></span><span class="line">                </span></span>
<span class="srcline"><span class="lineno"><a href="1,265" id="srcline265">265</a></span><span class="line">                <span class="mxinfo" id="T12:U752"><span class="var type1" id="S24T12U1142">x_apo</span>=<span class="mxinfo" id="T12:U754"><span class="var type1" id="S51T12U1144">x_apr</span>+<span class="mxinfo" id="T12:U756"><span class="var type1" id="S65T42U1146">K_k</span>*<span class="var type1" id="S62T41U1147">y_k</span></span></span></span>;</span></span>
<span class="srcline"><span class="lineno"><a href="1,266" id="srcline266">266</a></span><span class="line">                <span class="mxinfo" id="T24:U759"><span class="var type1" id="S29T24U1150">P_apo</span>=<span class="mxinfo" id="T24:U761">(<span class="mxinfo" id="T24:U762"><span class="mxinfo" id="T11:U763">eye(12)</span>-<span class="mxinfo" id="T24:U764"><span class="var type1" id="S65T42U1158">K_k</span>*<span class="mxinfo" id="T40:U766"><span class="var type1" id="S61T40U1160">H_k</span>(1:6,1:12)</span></span></span>)*<span class="var type1" id="S59T24U1167">P_apr</span></span></span>;</span></span>
<span class="srcline"><span class="lineno"><a href="1,267" id="srcline267">267</a></span><span class="line">            <span class="keyword">else</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,268" id="srcline268">268</a></span><span class="line">                <span class="mxinfo" id="T12:U769"><span class="var type1" id="S24T12U1171">x_apo</span>=<span class="var type1" id="S51T12U1172">x_apr</span></span>;</span></span>
<span class="srcline"><span class="lineno"><a href="1,269" id="srcline269">269</a></span><span class="line">                <span class="mxinfo" id="T24:U772"><span class="var type1" id="S29T24U1175">P_apo</span>=<span class="var type1" id="S59T24U1176">P_apr</span></span>;</span></span>
<span class="srcline"><span class="lineno"><a href="1,270" id="srcline270">270</a></span><span class="line">            <span class="keyword">end</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,271" id="srcline271">271</a></span><span class="line">        <span class="keyword">end</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,272" id="srcline272">272</a></span><span class="line">    <span class="keyword">end</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,273" id="srcline273">273</a></span><span class="line"><span class="keyword">end</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,274" id="srcline274">274</a></span><span class="line"></span></span>
<span class="srcline"><span class="lineno"><a href="1,275" id="srcline275">275</a></span><span class="line"></span></span>
<span class="srcline"><span class="lineno"><a href="1,276" id="srcline276">276</a></span><span class="line"></span></span>
<span class="srcline"><span class="lineno"><a href="1,277" id="srcline277">277</a></span><span class="line"><span class="comment">%% euler anglels extraction</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,278" id="srcline278">278</a></span><span class="line"><span class="mxinfo" id="T10:U775"><span class="var type1" id="S67T10U1179">z_n_b</span> = <span class="mxinfo" id="T10:U777"><span class="mxinfo" id="T10:U778">-<span class="mxinfo" id="T10:U779"><span class="var type1" id="S24T12U1183">x_apo</span>(<span class="mxinfo" id="T49:U781"><span class="mxinfo" id="T6:U782">7</span>:<span class="mxinfo" id="T6:U783">9</span></span>)</span></span>./<span class="mxinfo" id="T5:U784">norm(<span class="mxinfo" id="T10:U785"><span class="var type1" id="S24T12U1190">x_apo</span>(<span class="mxinfo" id="T49:U787"><span class="mxinfo" id="T6:U788">7</span>:<span class="mxinfo" id="T6:U789">9</span></span>)</span>)</span></span></span>;</span></span>
<span class="srcline"><span class="lineno"><a href="1,279" id="srcline279">279</a></span><span class="line"><span class="mxinfo" id="T10:U790"><span class="var type1" id="S69T10U1196">m_n_b</span> = <span class="mxinfo" id="T10:U792"><span class="mxinfo" id="T10:U793"><span class="var type1" id="S24T12U1199">x_apo</span>(<span class="mxinfo" id="T49:U795"><span class="mxinfo" id="T6:U796">10</span>:<span class="mxinfo" id="T6:U797">12</span></span>)</span>./<span class="mxinfo" id="T5:U798">norm(<span class="mxinfo" id="T10:U799"><span class="var type1" id="S24T12U1206">x_apo</span>(<span class="mxinfo" id="T49:U801"><span class="mxinfo" id="T6:U802">10</span>:<span class="mxinfo" id="T6:U803">12</span></span>)</span>)</span></span></span>;</span></span>
<span class="srcline"><span class="lineno"><a href="1,280" id="srcline280">280</a></span><span class="line"></span></span>
<span class="srcline"><span class="lineno"><a href="1,281" id="srcline281">281</a></span><span class="line"><span class="mxinfo" id="T10:U804"><span class="var type1" id="S70T10U1212">y_n_b</span>=<span class="mxinfo" id="T10:U806">cross(<span class="var type1" id="S67T10U1215">z_n_b</span>,<span class="var type1" id="S69T10U1216">m_n_b</span>)</span></span>;</span></span>
<span class="srcline"><span class="lineno"><a href="1,282" id="srcline282">282</a></span><span class="line"><span class="mxinfo" id="T10:U809"><span class="var type1" id="S70T10U1219">y_n_b</span>=<span class="mxinfo" id="T10:U811"><span class="var type1" id="S70T10U1221">y_n_b</span>./<span class="mxinfo" id="T5:U813">norm(<span class="var type1" id="S70T10U1224">y_n_b</span>)</span></span></span>;</span></span>
<span class="srcline"><span class="lineno"><a href="1,283" id="srcline283">283</a></span><span class="line"></span></span>
<span class="srcline"><span class="lineno"><a href="1,284" id="srcline284">284</a></span><span class="line"><span class="mxinfo" id="T10:U815"><span class="var type1" id="S71T10U1227">x_n_b</span>=(<span class="mxinfo" id="T10:U817">cross(<span class="var type1" id="S70T10U1231">y_n_b</span>,<span class="var type1" id="S67T10U1232">z_n_b</span>)</span>)</span>;</span></span>
<span class="srcline"><span class="lineno"><a href="1,285" id="srcline285">285</a></span><span class="line"><span class="mxinfo" id="T10:U820"><span class="var type1" id="S71T10U1235">x_n_b</span>=<span class="mxinfo" id="T10:U822"><span class="var type1" id="S71T10U1237">x_n_b</span>./<span class="mxinfo" id="T5:U824">norm(<span class="var type1" id="S71T10U1240">x_n_b</span>)</span></span></span>;</span></span>
<span class="srcline"><span class="lineno"><a href="1,286" id="srcline286">286</a></span><span class="line"></span></span>
<span class="srcline"><span class="lineno"><a href="1,287" id="srcline287">287</a></span><span class="line"></span></span>
<span class="srcline"><span class="lineno"><a href="1,288" id="srcline288">288</a></span><span class="line"><span class="mxinfo" id="T12:U826"><span class="var type1" id="S2T12U1243">xa_apo</span>=<span class="var type1" id="S24T12U1244">x_apo</span></span>;</span></span>
<span class="srcline"><span class="lineno"><a href="1,289" id="srcline289">289</a></span><span class="line"><span class="mxinfo" id="T24:U829"><span class="var type1" id="S3T24U1247">Pa_apo</span>=<span class="var type1" id="S29T24U1248">P_apo</span></span>;</span></span>
<span class="srcline"><span class="lineno"><a href="1,290" id="srcline290">290</a></span><span class="line"><span class="comment">% rotation matrix from earth to body system</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,291" id="srcline291">291</a></span><span class="line"><span class="mxinfo" id="T2:U832"><span class="var type1" id="S4T2U1251">Rot_matrix</span>=<span class="mxinfo" id="T2:U834">[<span class="var type1" id="S71T10U1254">x_n_b</span>,<span class="var type1" id="S70T10U1255">y_n_b</span>,<span class="var type1" id="S67T10U1256">z_n_b</span>]</span></span>;</span></span>
<span class="srcline"><span class="lineno"><a href="1,292" id="srcline292">292</a></span><span class="line"></span></span>
<span class="srcline"><span class="lineno"><a href="1,293" id="srcline293">293</a></span><span class="line"></span></span>
<span class="srcline"><span class="lineno"><a href="1,294" id="srcline294">294</a></span><span class="line"><span class="mxinfo" id="T5:U838"><span class="var type1" id="S72T5U1259">phi</span>=<span class="mxinfo" id="T5:U840">atan2(<span class="mxinfo" id="T5:U841"><span class="var type1" id="S4T2U1263">Rot_matrix</span>(<span class="mxinfo" id="T6:U843">2</span>,<span class="mxinfo" id="T6:U844">3</span>)</span>,<span class="mxinfo" id="T5:U845"><span class="var type1" id="S4T2U1267">Rot_matrix</span>(<span class="mxinfo" id="T6:U847">3</span>,<span class="mxinfo" id="T6:U848">3</span>)</span>)</span></span>;</span></span>
<span class="srcline"><span class="lineno"><a href="1,295" id="srcline295">295</a></span><span class="line"><span class="mxinfo" id="T5:U849"><span class="var type1" id="S74T5U1272">theta</span>=<span class="mxinfo" id="T5:U851">-<span class="mxinfo" id="T5:U852">asin(<span class="mxinfo" id="T5:U853"><span class="var type1" id="S4T2U1277">Rot_matrix</span>(<span class="mxinfo" id="T6:U855">1</span>,<span class="mxinfo" id="T6:U856">3</span>)</span>)</span></span></span>;</span></span>
<span class="srcline"><span class="lineno"><a href="1,296" id="srcline296">296</a></span><span class="line"><span class="mxinfo" id="T5:U857"><span class="var type1" id="S76T5U1282">psi</span>=<span class="mxinfo" id="T5:U859">atan2(<span class="mxinfo" id="T5:U860"><span class="var type1" id="S4T2U1286">Rot_matrix</span>(<span class="mxinfo" id="T6:U862">1</span>,<span class="mxinfo" id="T6:U863">2</span>)</span>,<span class="mxinfo" id="T5:U864"><span class="var type1" id="S4T2U1290">Rot_matrix</span>(<span class="mxinfo" id="T6:U866">1</span>,<span class="mxinfo" id="T6:U867">1</span>)</span>)</span></span>;</span></span>
<span class="srcline"><span class="lineno"><a href="1,297" id="srcline297">297</a></span><span class="line"><span class="mxinfo" id="T10:U868"><span class="var type1" id="S5T10U1295">eulerAngles</span>=<span class="mxinfo" id="T10:U870">[<span class="var type1" id="S72T5U1298">phi</span>;<span class="var type1" id="S74T5U1300">theta</span>;<span class="var type1" id="S76T5U1302">psi</span>]</span></span>;</span></span>
<span class="srcline"><span class="lineno"><a href="1,298" id="srcline298">298</a></span><span class="line"></span></span>
</pre>
